// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2008-2014 Gael Guennebaud <gael.guennebaud@inria.fr>
// Copyright (C) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#ifndef SVD_DEFAULT
#error a macro SVD_DEFAULT(MatrixType) must be defined prior to including svd_common.h
#endif

#ifndef SVD_FOR_MIN_NORM
#error a macro SVD_FOR_MIN_NORM(MatrixType) must be defined prior to including svd_common.h
#endif

#include "solverbase.h"
#include "svd_fill.h"

// Check that the matrix m is properly reconstructed and that the U and V factors are unitary
// The SVD must have already been computed.
template<typename SvdType, typename MatrixType>
void
svd_check_full(const MatrixType& m, const SvdType& svd)
{
	Index rows = m.rows();
	Index cols = m.cols();

	enum
	{
		RowsAtCompileTime = MatrixType::RowsAtCompileTime,
		ColsAtCompileTime = MatrixType::ColsAtCompileTime
	};

	typedef typename MatrixType::Scalar Scalar;
	typedef typename MatrixType::RealScalar RealScalar;
	typedef Matrix<Scalar, RowsAtCompileTime, RowsAtCompileTime> MatrixUType;
	typedef Matrix<Scalar, ColsAtCompileTime, ColsAtCompileTime> MatrixVType;

	MatrixType sigma = MatrixType::Zero(rows, cols);
	sigma.diagonal() = svd.singularValues().template cast<Scalar>();
	MatrixUType u = svd.matrixU();
	MatrixVType v = svd.matrixV();
	RealScalar scaling = m.cwiseAbs().maxCoeff();
	if (scaling < (std::numeric_limits<RealScalar>::min)()) {
		VERIFY(sigma.cwiseAbs().maxCoeff() <= (std::numeric_limits<RealScalar>::min)());
	} else {
		VERIFY_IS_APPROX(m / scaling, u * (sigma / scaling) * v.adjoint());
	}
	VERIFY_IS_UNITARY(u);
	VERIFY_IS_UNITARY(v);
}

// Compare partial SVD defined by computationOptions to a full SVD referenceSvd
template<typename SvdType, typename MatrixType>
void
svd_compare_to_full(const MatrixType& m, unsigned int computationOptions, const SvdType& referenceSvd)
{
	typedef typename MatrixType::RealScalar RealScalar;
	Index rows = m.rows();
	Index cols = m.cols();
	Index diagSize = (std::min)(rows, cols);
	RealScalar prec = test_precision<RealScalar>();

	SvdType svd(m, computationOptions);

	VERIFY_IS_APPROX(svd.singularValues(), referenceSvd.singularValues());

	if (computationOptions & (ComputeFullV | ComputeThinV)) {
		VERIFY((svd.matrixV().adjoint() * svd.matrixV()).isIdentity(prec));
		VERIFY_IS_APPROX(svd.matrixV().leftCols(diagSize) * svd.singularValues().asDiagonal() *
							 svd.matrixV().leftCols(diagSize).adjoint(),
						 referenceSvd.matrixV().leftCols(diagSize) * referenceSvd.singularValues().asDiagonal() *
							 referenceSvd.matrixV().leftCols(diagSize).adjoint());
	}

	if (computationOptions & (ComputeFullU | ComputeThinU)) {
		VERIFY((svd.matrixU().adjoint() * svd.matrixU()).isIdentity(prec));
		VERIFY_IS_APPROX(svd.matrixU().leftCols(diagSize) * svd.singularValues().cwiseAbs2().asDiagonal() *
							 svd.matrixU().leftCols(diagSize).adjoint(),
						 referenceSvd.matrixU().leftCols(diagSize) *
							 referenceSvd.singularValues().cwiseAbs2().asDiagonal() *
							 referenceSvd.matrixU().leftCols(diagSize).adjoint());
	}

	// The following checks are not critical.
	// For instance, with Dived&Conquer SVD, if only the factor 'V' is computedt then different matrix-matrix product
	// implementation will be used and the resulting 'V' factor might be significantly different when the SVD
	// decomposition is not unique, especially with single precision float.
	++g_test_level;
	if (computationOptions & ComputeFullU)
		VERIFY_IS_APPROX(svd.matrixU(), referenceSvd.matrixU());
	if (computationOptions & ComputeThinU)
		VERIFY_IS_APPROX(svd.matrixU(), referenceSvd.matrixU().leftCols(diagSize));
	if (computationOptions & ComputeFullV)
		VERIFY_IS_APPROX(svd.matrixV().cwiseAbs(), referenceSvd.matrixV().cwiseAbs());
	if (computationOptions & ComputeThinV)
		VERIFY_IS_APPROX(svd.matrixV(), referenceSvd.matrixV().leftCols(diagSize));
	--g_test_level;
}

//
template<typename SvdType, typename MatrixType>
void
svd_least_square(const MatrixType& m, unsigned int computationOptions)
{
	typedef typename MatrixType::Scalar Scalar;
	typedef typename MatrixType::RealScalar RealScalar;
	Index rows = m.rows();
	Index cols = m.cols();

	enum
	{
		RowsAtCompileTime = MatrixType::RowsAtCompileTime,
		ColsAtCompileTime = MatrixType::ColsAtCompileTime
	};

	typedef Matrix<Scalar, RowsAtCompileTime, Dynamic> RhsType;
	typedef Matrix<Scalar, ColsAtCompileTime, Dynamic> SolutionType;

	RhsType rhs = RhsType::Random(rows, internal::random<Index>(1, cols));
	SvdType svd(m, computationOptions);

	if (internal::is_same<RealScalar, double>::value)
		svd.setThreshold(1e-8);
	else if (internal::is_same<RealScalar, float>::value)
		svd.setThreshold(2e-4);

	SolutionType x = svd.solve(rhs);

	RealScalar residual = (m * x - rhs).norm();
	RealScalar rhs_norm = rhs.norm();
	if (!test_isMuchSmallerThan(residual, rhs.norm())) {
		// ^^^ If the residual is very small, then we have an exact solution, so we are already good.

		// evaluate normal equation which works also for least-squares solutions
		if (internal::is_same<RealScalar, double>::value || svd.rank() == m.diagonal().size()) {
			using std::sqrt;
			// This test is not stable with single precision.
			// This is probably because squaring m signicantly affects the precision.
			if (internal::is_same<RealScalar, float>::value)
				++g_test_level;

			VERIFY_IS_APPROX(m.adjoint() * (m * x), m.adjoint() * rhs);

			if (internal::is_same<RealScalar, float>::value)
				--g_test_level;
		}

		// Check that there is no significantly better solution in the neighborhood of x
		for (Index k = 0; k < x.rows(); ++k) {
			using std::abs;

			SolutionType y(x);
			y.row(k) = (RealScalar(1) + 2 * NumTraits<RealScalar>::epsilon()) * x.row(k);
			RealScalar residual_y = (m * y - rhs).norm();
			VERIFY(test_isMuchSmallerThan(abs(residual_y - residual), rhs_norm) || residual < residual_y);
			if (internal::is_same<RealScalar, float>::value)
				++g_test_level;
			VERIFY(test_isApprox(residual_y, residual) || residual < residual_y);
			if (internal::is_same<RealScalar, float>::value)
				--g_test_level;

			y.row(k) = (RealScalar(1) - 2 * NumTraits<RealScalar>::epsilon()) * x.row(k);
			residual_y = (m * y - rhs).norm();
			VERIFY(test_isMuchSmallerThan(abs(residual_y - residual), rhs_norm) || residual < residual_y);
			if (internal::is_same<RealScalar, float>::value)
				++g_test_level;
			VERIFY(test_isApprox(residual_y, residual) || residual < residual_y);
			if (internal::is_same<RealScalar, float>::value)
				--g_test_level;
		}
	}
}

// check minimal norm solutions, the inoput matrix m is only used to recover problem size
template<typename MatrixType>
void
svd_min_norm(const MatrixType& m, unsigned int computationOptions)
{
	typedef typename MatrixType::Scalar Scalar;
	Index cols = m.cols();

	enum
	{
		ColsAtCompileTime = MatrixType::ColsAtCompileTime
	};

	typedef Matrix<Scalar, ColsAtCompileTime, Dynamic> SolutionType;

	// generate a full-rank m x n problem with m<n
	enum
	{
		RankAtCompileTime2 = ColsAtCompileTime == Dynamic ? Dynamic : (ColsAtCompileTime) / 2 + 1,
		RowsAtCompileTime3 = ColsAtCompileTime == Dynamic ? Dynamic : ColsAtCompileTime + 1
	};
	typedef Matrix<Scalar, RankAtCompileTime2, ColsAtCompileTime> MatrixType2;
	typedef Matrix<Scalar, RankAtCompileTime2, 1> RhsType2;
	typedef Matrix<Scalar, ColsAtCompileTime, RankAtCompileTime2> MatrixType2T;
	Index rank = RankAtCompileTime2 == Dynamic ? internal::random<Index>(1, cols) : Index(RankAtCompileTime2);
	MatrixType2 m2(rank, cols);
	int guard = 0;
	do {
		m2.setRandom();
	} while (SVD_FOR_MIN_NORM(MatrixType2)(m2).setThreshold(test_precision<Scalar>()).rank() != rank && (++guard) < 10);
	VERIFY(guard < 10);

	RhsType2 rhs2 = RhsType2::Random(rank);
	// use QR to find a reference minimal norm solution
	HouseholderQR<MatrixType2T> qr(m2.adjoint());
	Matrix<Scalar, Dynamic, 1> tmp =
		qr.matrixQR().topLeftCorner(rank, rank).template triangularView<Upper>().adjoint().solve(rhs2);
	tmp.conservativeResize(cols);
	tmp.tail(cols - rank).setZero();
	SolutionType x21 = qr.householderQ() * tmp;
	// now check with SVD
	SVD_FOR_MIN_NORM(MatrixType2) svd2(m2, computationOptions);
	SolutionType x22 = svd2.solve(rhs2);
	VERIFY_IS_APPROX(m2 * x21, rhs2);
	VERIFY_IS_APPROX(m2 * x22, rhs2);
	VERIFY_IS_APPROX(x21, x22);

	// Now check with a rank deficient matrix
	typedef Matrix<Scalar, RowsAtCompileTime3, ColsAtCompileTime> MatrixType3;
	typedef Matrix<Scalar, RowsAtCompileTime3, 1> RhsType3;
	Index rows3 =
		RowsAtCompileTime3 == Dynamic ? internal::random<Index>(rank + 1, 2 * cols) : Index(RowsAtCompileTime3);
	Matrix<Scalar, RowsAtCompileTime3, Dynamic> C = Matrix<Scalar, RowsAtCompileTime3, Dynamic>::Random(rows3, rank);
	MatrixType3 m3 = C * m2;
	RhsType3 rhs3 = C * rhs2;
	SVD_FOR_MIN_NORM(MatrixType3) svd3(m3, computationOptions);
	SolutionType x3 = svd3.solve(rhs3);
	VERIFY_IS_APPROX(m3 * x3, rhs3);
	VERIFY_IS_APPROX(m3 * x21, rhs3);
	VERIFY_IS_APPROX(m2 * x3, rhs2);
	VERIFY_IS_APPROX(x21, x3);
}

template<typename MatrixType, typename SolverType>
void
svd_test_solvers(const MatrixType& m, const SolverType& solver)
{
	Index rows, cols, cols2;

	rows = m.rows();
	cols = m.cols();

	if (MatrixType::ColsAtCompileTime == Dynamic) {
		cols2 = internal::random<int>(2, EIGEN_TEST_MAX_SIZE);
	} else {
		cols2 = cols;
	}
	typedef Matrix<typename MatrixType::Scalar, MatrixType::ColsAtCompileTime, MatrixType::ColsAtCompileTime>
		CMatrixType;
	check_solverbase<CMatrixType, MatrixType>(m, solver, rows, cols, cols2);
}

// Check full, compare_to_full, least_square, and min_norm for all possible compute-options
template<typename SvdType, typename MatrixType>
void
svd_test_all_computation_options(const MatrixType& m, bool full_only)
{
	//   if (QRPreconditioner == NoQRPreconditioner && m.rows() != m.cols())
	//     return;
	STATIC_CHECK((internal::is_same<typename SvdType::StorageIndex, int>::value));

	SvdType fullSvd(m, ComputeFullU | ComputeFullV);
	CALL_SUBTEST((svd_check_full(m, fullSvd)));
	CALL_SUBTEST((svd_least_square<SvdType>(m, ComputeFullU | ComputeFullV)));
	CALL_SUBTEST((svd_min_norm(m, ComputeFullU | ComputeFullV)));

#if defined __INTEL_COMPILER
// remark #111: statement is unreachable
#pragma warning disable 111
#endif

	svd_test_solvers(m, fullSvd);

	if (full_only)
		return;

	CALL_SUBTEST((svd_compare_to_full(m, ComputeFullU, fullSvd)));
	CALL_SUBTEST((svd_compare_to_full(m, ComputeFullV, fullSvd)));
	CALL_SUBTEST((svd_compare_to_full(m, 0, fullSvd)));

	if (MatrixType::ColsAtCompileTime == Dynamic) {
		// thin U/V are only available with dynamic number of columns
		CALL_SUBTEST((svd_compare_to_full(m, ComputeFullU | ComputeThinV, fullSvd)));
		CALL_SUBTEST((svd_compare_to_full(m, ComputeThinV, fullSvd)));
		CALL_SUBTEST((svd_compare_to_full(m, ComputeThinU | ComputeFullV, fullSvd)));
		CALL_SUBTEST((svd_compare_to_full(m, ComputeThinU, fullSvd)));
		CALL_SUBTEST((svd_compare_to_full(m, ComputeThinU | ComputeThinV, fullSvd)));

		CALL_SUBTEST((svd_least_square<SvdType>(m, ComputeFullU | ComputeThinV)));
		CALL_SUBTEST((svd_least_square<SvdType>(m, ComputeThinU | ComputeFullV)));
		CALL_SUBTEST((svd_least_square<SvdType>(m, ComputeThinU | ComputeThinV)));

		CALL_SUBTEST((svd_min_norm(m, ComputeFullU | ComputeThinV)));
		CALL_SUBTEST((svd_min_norm(m, ComputeThinU | ComputeFullV)));
		CALL_SUBTEST((svd_min_norm(m, ComputeThinU | ComputeThinV)));

		// test reconstruction
		Index diagSize = (std::min)(m.rows(), m.cols());
		SvdType svd(m, ComputeThinU | ComputeThinV);
		VERIFY_IS_APPROX(m,
						 svd.matrixU().leftCols(diagSize) * svd.singularValues().asDiagonal() *
							 svd.matrixV().leftCols(diagSize).adjoint());
	}
}

// work around stupid msvc error when constructing at compile time an expression that involves
// a division by zero, even if the numeric type has floating point
template<typename Scalar>
EIGEN_DONT_INLINE Scalar
zero()
{
	return Scalar(0);
}

// workaround aggressive optimization in ICC
template<typename T>
EIGEN_DONT_INLINE T
sub(T a, T b)
{
	return a - b;
}

// This function verifies we don't iterate infinitely on nan/inf values,
// and that info() returns InvalidInput.
template<typename SvdType, typename MatrixType>
void
svd_inf_nan()
{
	SvdType svd;
	typedef typename MatrixType::Scalar Scalar;
	Scalar some_inf = Scalar(1) / zero<Scalar>();
	VERIFY(sub(some_inf, some_inf) != sub(some_inf, some_inf));
	svd.compute(MatrixType::Constant(10, 10, some_inf), ComputeFullU | ComputeFullV);
	VERIFY(svd.info() == InvalidInput);

	Scalar nan = std::numeric_limits<Scalar>::quiet_NaN();
	VERIFY(nan != nan);
	svd.compute(MatrixType::Constant(10, 10, nan), ComputeFullU | ComputeFullV);
	VERIFY(svd.info() == InvalidInput);

	MatrixType m = MatrixType::Zero(10, 10);
	m(internal::random<int>(0, 9), internal::random<int>(0, 9)) = some_inf;
	svd.compute(m, ComputeFullU | ComputeFullV);
	VERIFY(svd.info() == InvalidInput);

	m = MatrixType::Zero(10, 10);
	m(internal::random<int>(0, 9), internal::random<int>(0, 9)) = nan;
	svd.compute(m, ComputeFullU | ComputeFullV);
	VERIFY(svd.info() == InvalidInput);

	// regression test for bug 791
	m.resize(3, 3);
	m << 0, 2 * NumTraits<Scalar>::epsilon(), 0.5, 0, -0.5, 0, nan, 0, 0;
	svd.compute(m, ComputeFullU | ComputeFullV);
	VERIFY(svd.info() == InvalidInput);

	m.resize(4, 4);
	m << 1, 0, 0, 0, 0, 3, 1, 2e-308, 1, 0, 1, nan, 0, nan, nan, 0;
	svd.compute(m, ComputeFullU | ComputeFullV);
	VERIFY(svd.info() == InvalidInput);
}

// Regression test for bug 286: JacobiSVD loops indefinitely with some
// matrices containing denormal numbers.
template<typename>
void
svd_underoverflow()
{
#if defined __INTEL_COMPILER
// shut up warning #239: floating point underflow
#pragma warning push
#pragma warning disable 239
#endif
	Matrix2d M;
	M << -7.90884e-313, -4.94e-324, 0, 5.60844e-313;
	SVD_DEFAULT(Matrix2d) svd;
	svd.compute(M, ComputeFullU | ComputeFullV);
	CALL_SUBTEST(svd_check_full(M, svd));

	// Check all 2x2 matrices made with the following coefficients:
	VectorXd value_set(9);
	value_set << 0, 1, -1, 5.60844e-313, -5.60844e-313, 4.94e-324, -4.94e-324, -4.94e-223, 4.94e-223;
	Array4i id(0, 0, 0, 0);
	int k = 0;
	do {
		M << value_set(id(0)), value_set(id(1)), value_set(id(2)), value_set(id(3));
		svd.compute(M, ComputeFullU | ComputeFullV);
		CALL_SUBTEST(svd_check_full(M, svd));

		id(k)++;
		if (id(k) >= value_set.size()) {
			while (k < 3 && id(k) >= value_set.size())
				id(++k)++;
			id.head(k).setZero();
			k = 0;
		}

	} while ((id < int(value_set.size())).all());

#if defined __INTEL_COMPILER
#pragma warning pop
#endif

	// Check for overflow:
	Matrix3d M3;
	M3 << 4.4331978442502944e+307, -5.8585363752028680e+307, 6.4527017443412964e+307, 3.7841695601406358e+307,
		2.4331702789740617e+306, -3.5235707140272905e+307, -8.7190887618028355e+307, -7.3453213709232193e+307,
		-2.4367363684472105e+307;

	SVD_DEFAULT(Matrix3d) svd3;
	svd3.compute(M3, ComputeFullU | ComputeFullV); // just check we don't loop indefinitely
	CALL_SUBTEST(svd_check_full(M3, svd3));
}

// void jacobisvd(const MatrixType& a = MatrixType(), bool pickrandom = true)

template<typename MatrixType>
void
svd_all_trivial_2x2(void (*cb)(const MatrixType&, bool))
{
	MatrixType M;
	VectorXd value_set(3);
	value_set << 0, 1, -1;
	Array4i id(0, 0, 0, 0);
	int k = 0;
	do {
		M << value_set(id(0)), value_set(id(1)), value_set(id(2)), value_set(id(3));

		cb(M, false);

		id(k)++;
		if (id(k) >= value_set.size()) {
			while (k < 3 && id(k) >= value_set.size())
				id(++k)++;
			id.head(k).setZero();
			k = 0;
		}

	} while ((id < int(value_set.size())).all());
}

template<typename>
void
svd_preallocate()
{
	Vector3f v(3.f, 2.f, 1.f);
	MatrixXf m = v.asDiagonal();

	internal::set_is_malloc_allowed(false);
	VERIFY_RAISES_ASSERT(VectorXf tmp(10);)
	SVD_DEFAULT(MatrixXf) svd;
	internal::set_is_malloc_allowed(true);
	svd.compute(m);
	VERIFY_IS_APPROX(svd.singularValues(), v);

	SVD_DEFAULT(MatrixXf) svd2(3, 3);
	internal::set_is_malloc_allowed(false);
	svd2.compute(m);
	internal::set_is_malloc_allowed(true);
	VERIFY_IS_APPROX(svd2.singularValues(), v);
	VERIFY_RAISES_ASSERT(svd2.matrixU());
	VERIFY_RAISES_ASSERT(svd2.matrixV());
	svd2.compute(m, ComputeFullU | ComputeFullV);
	VERIFY_IS_APPROX(svd2.matrixU(), Matrix3f::Identity());
	VERIFY_IS_APPROX(svd2.matrixV(), Matrix3f::Identity());
	internal::set_is_malloc_allowed(false);
	svd2.compute(m);
	internal::set_is_malloc_allowed(true);

	SVD_DEFAULT(MatrixXf) svd3(3, 3, ComputeFullU | ComputeFullV);
	internal::set_is_malloc_allowed(false);
	svd2.compute(m);
	internal::set_is_malloc_allowed(true);
	VERIFY_IS_APPROX(svd2.singularValues(), v);
	VERIFY_IS_APPROX(svd2.matrixU(), Matrix3f::Identity());
	VERIFY_IS_APPROX(svd2.matrixV(), Matrix3f::Identity());
	internal::set_is_malloc_allowed(false);
	svd2.compute(m, ComputeFullU | ComputeFullV);
	internal::set_is_malloc_allowed(true);
}

template<typename SvdType, typename MatrixType>
void
svd_verify_assert(const MatrixType& m, bool fullOnly = false)
{
	typedef typename MatrixType::Scalar Scalar;
	Index rows = m.rows();
	Index cols = m.cols();

	enum
	{
		RowsAtCompileTime = MatrixType::RowsAtCompileTime,
		ColsAtCompileTime = MatrixType::ColsAtCompileTime
	};

	typedef Matrix<Scalar, RowsAtCompileTime, 1> RhsType;
	RhsType rhs(rows);
	SvdType svd;
	VERIFY_RAISES_ASSERT(svd.matrixU())
	VERIFY_RAISES_ASSERT(svd.singularValues())
	VERIFY_RAISES_ASSERT(svd.matrixV())
	VERIFY_RAISES_ASSERT(svd.solve(rhs))
	VERIFY_RAISES_ASSERT(svd.transpose().solve(rhs))
	VERIFY_RAISES_ASSERT(svd.adjoint().solve(rhs))
	MatrixType a = MatrixType::Zero(rows, cols);
	a.setZero();
	svd.compute(a, 0);
	VERIFY_RAISES_ASSERT(svd.matrixU())
	VERIFY_RAISES_ASSERT(svd.matrixV())
	svd.singularValues();
	VERIFY_RAISES_ASSERT(svd.solve(rhs))

	svd.compute(a, ComputeFullU);
	svd.matrixU();
	VERIFY_RAISES_ASSERT(svd.matrixV())
	VERIFY_RAISES_ASSERT(svd.solve(rhs))
	svd.compute(a, ComputeFullV);
	svd.matrixV();
	VERIFY_RAISES_ASSERT(svd.matrixU())
	VERIFY_RAISES_ASSERT(svd.solve(rhs))

	if (!fullOnly && ColsAtCompileTime == Dynamic) {
		svd.compute(a, ComputeThinU);
		svd.matrixU();
		VERIFY_RAISES_ASSERT(svd.matrixV())
		VERIFY_RAISES_ASSERT(svd.solve(rhs))
		svd.compute(a, ComputeThinV);
		svd.matrixV();
		VERIFY_RAISES_ASSERT(svd.matrixU())
		VERIFY_RAISES_ASSERT(svd.solve(rhs))
	} else {
		VERIFY_RAISES_ASSERT(svd.compute(a, ComputeThinU))
		VERIFY_RAISES_ASSERT(svd.compute(a, ComputeThinV))
	}
}

#undef SVD_DEFAULT
#undef SVD_FOR_MIN_NORM
